#include <basketball_filter/sensor_sync_checker.h>


SensorSyncCheker::SensorSyncCheker() :
    is_first_sync(true),
    drops(0),
    frame_cnt(0)
{
    //Grab parameters from parameter sever
    private_nh_.param("use_imu", use_imu, false);
    private_nh_.param("rgb_img_topic", rgb_img_topic, std::string(""));
    private_nh_.param("depth_img_topic", depth_img_topic, std::string(""));
    private_nh_.param("report_thresh", report_thresh_str, 0.);

    private_nh_.param("csv_path", csv_path, std::string(""));
    report_thresh = ros::Duration(report_thresh_str);

    // The csv file
    ofs.open(csv_path);

    message_filters::Subscriber<sensor_msgs::Image> rgb_img_sub_(private_nh_, rgb_img_topic, 20);
    message_filters::Subscriber<sensor_msgs::Image> depth_img_sub_(private_nh_, depth_img_topic, 20);
    if (use_imu)
    {
        private_nh_.param("imu_topic", imu_topic, std::string(""));
        message_filters::Subscriber<sensor_msgs::Imu> imu_sub_(private_nh_, imu_topic, 20);
        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
                                                                sensor_msgs::Image,
                                                                sensor_msgs::Imu> myPolicy;
        message_filters::Synchronizer<myPolicy> synchronizer(myPolicy(30),
                                                             rgb_img_sub_,
                                                             depth_img_sub_,
                                                             imu_sub_);
        synchronizer.registerCallback(boost::bind(&SensorSyncCheker::syncCB2, this, _1, _2, _3));
        ros::spin();
    }


    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
                                                            sensor_msgs::Image> myPolicy;

    message_filters::Synchronizer<myPolicy> synchronizer(myPolicy(30),
                                                         rgb_img_sub_,
                                                         depth_img_sub_);
    synchronizer.registerCallback(boost::bind(&SensorSyncCheker::syncCB1, this, _1, _2));
    ros::spin();
}

SensorSyncCheker::~SensorSyncCheker() { this->summarize(); }

void SensorSyncCheker::syncCB1(const sensor_msgs::ImageConstPtr &rgb_img_msg,
                          const sensor_msgs::ImageConstPtr &depth_img_msg)
{
    if (is_first_sync)
    {
        is_first_sync = false;
        last_sync = ros::Time::now();
        total_diff = ros::Duration(0.0);
    }
    else
    {
        auto frame_diff = ros::Time::now() - last_sync;
        ofs << frame_diff << ",";
        last_sync = ros::Time::now();
        ROS_INFO_STREAM("frame diff: " << frame_diff);
        if (frame_diff >= ros::Duration(report_thresh))
        {
            ++drops;
        }
        total_diff += frame_diff;
    }
    ++frame_cnt;
}

void SensorSyncCheker::syncCB2(const sensor_msgs::ImageConstPtr &rgb_img_msg,
                          const sensor_msgs::ImageConstPtr &depth_img_msg,
                          const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (is_first_sync)
    {
        is_first_sync = false;
        last_sync = ros::Time::now();
        total_diff = ros::Duration(0.0);
    }
    else
    {
        auto frame_diff = ros::Time::now() - last_sync;
        ofs << frame_diff << ",";
        last_sync = ros::Time::now();
        ROS_INFO_STREAM("frame diff: " << frame_diff);
        if (frame_diff >= ros::Duration(report_thresh))
        {
            ++drops;
        }
        total_diff += frame_diff;
    }
    ++frame_cnt;
}

void SensorSyncCheker::summarize()
{
    std::cout << drops << " frames over the threshold!" << std::endl;
    std::cout << "Total " << frame_cnt << " frames" << std::endl;
    std::cout << "Average fps: " << total_diff.toSec() / frame_cnt << std::endl;
    ofs.close();
}
